#include <ros/ros.h>
#include <atomic>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <std_msgs/UInt64.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Int32MultiArray.h>

#include "unionsys_core.hpp"

#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <visualization_msgs/Marker.h>


#include <string>

#include <iostream>
#include <cstring>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <vector>
#include <unistd.h>
#include <typeinfo>
#include <stdlib.h>

#include "UDP.h"
#include "Quadrotor.h"

//接收dds消息，并发布到ros节点
using namespace std;

extern RegisterMSN *reg_sub;

UDPManager sfbd_manager;
Quadrotor sfbd_quadrotor;

ros::Publisher rospub_pose1;
ros::Publisher rospub_pose2;
ros::Publisher rospub_pose3;
ros::Publisher rospub_pose4;
ros::Publisher rospub_pose5;

void sfbd_recvFunction(const char *data, int length, int port) {
    PoseMsg msg;
    msg.deserialize(data, length);

    geometry_msgs::PoseStamped current_pose;
    current_pose.header.seq = atoi(msg.seq.c_str());
    current_pose.header.stamp = ros::Time::now();
    current_pose.header.frame_id = msg.frame_id;
    current_pose.pose.position.x = atof(msg.xPoint.c_str());
    current_pose.pose.position.y = atof(msg.yPoint.c_str());
    current_pose.pose.position.z = atof(msg.zPoint.c_str());
    current_pose.pose.orientation.x = atof(msg.xOrientation.c_str());
    current_pose.pose.orientation.y = atof(msg.yOrientation.c_str());
    current_pose.pose.orientation.z = atof(msg.zOrientation.c_str());
    current_pose.pose.orientation.w = atof(msg.wOrientation.c_str());

    if (port == sfbd_manager.BASE_PORT + 1)
        rospub_pose1.publish(current_pose);
    else if (port == sfbd_manager.BASE_PORT + 2)
        rospub_pose2.publish(current_pose);
    else if (port == sfbd_manager.BASE_PORT + 3)
        rospub_pose3.publish(current_pose);
    else if (port == sfbd_manager.BASE_PORT + 4)
        rospub_pose4.publish(current_pose);
    else if (port == sfbd_manager.BASE_PORT + 5)
        rospub_pose5.publish(current_pose);
    else
        std::cout << "err " << port << std::endl;
    std::cout << "port: " << port << std::endl;
    // std::cout << "Received message: " << std::endl;
    // std::cout << "Sequence: " << msg.seq << std::endl;
    // std::cout << "Frame ID: " << msg.frame_id << std::endl;
    // std::cout << "Point (x, y, z): (" << msg.xPoint << ", " << msg.yPoint << ", " << msg.zPoint << ")" << std::endl;
    // std::cout << "Orientation (x, y, z, w): (" << msg.xOrientation << ", " << msg.yOrientation << ", "
    //           << msg.zOrientation << ", " << msg.wOrientation << ")" << std::endl;
}

int main(int argc, char *argv[]) {
    reg_sub->detect_serial_number();
    ros::init(argc, argv, "SDPltDDSPosesub");
    ros::NodeHandle nodeHandle;

    // 设置循环的频率
    ros::Rate loop_rate(200);
    rospub_pose1 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone1/local_position/pose", 10);
    rospub_pose2 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone2/local_position/pose", 10);
    rospub_pose3 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone3/local_position/pose", 10);
    rospub_pose4 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone4/local_position/pose", 10);
    rospub_pose5 = nodeHandle.advertise<geometry_msgs::PoseStamped>("/drone5/local_position/pose", 10);

    int sfbd_quadrotorNum = sfbd_quadrotor.get_num();

    for (int i = 1; i <= sfbd_quadrotorNum; i++)
        sfbd_manager.addUDP(sfbd_manager.BASE_PORT + i, sfbd_recvFunction);

    sfbd_manager.startAll();

    //循环回调
    while (ros::ok() && nodeHandle.ok()) {
        ros::spinOnce();
        loop_rate.sleep();
    }
    nodeHandle.shutdown();
    return 0;
}
